﻿// Accord Math Library
// Accord.NET framework
// http://www.crsouza.com
//
// Copyright © César Souza, 2009-2010
// cesarsouza at gmail.com
//

namespace Accord.Math
{
    using System;
    using System.Collections.Generic;
    using System.Threading;
    


    class MatrixNullException : ApplicationException
    {
        public MatrixNullException() :
            base("To do this operation, matrix can not be null") { }
    }
    class MatrixNotSquare : ApplicationException
    {
        public MatrixNotSquare() :
            base("To do this operation, matrix must be a square matrix !") { }
    }
    class MatrixDeterminentZero : ApplicationException
    {
        public MatrixDeterminentZero() :
            base("Determinent of matrix equals zero, inverse can't be found !") { }
    }
    /// <summary>
    /// Static class Matrix. Defines a set of extension methods
    /// that operates mainly on multidimensional arrays and vectors.
    /// </summary>
    public static class Matrix
    {

        #region Comparison and Rounding
        /// <summary>
        ///   Compares two matrices for equality, considering an acceptance threshold.
        /// </summary>
        public static bool IsEqual(this double[,] a, double[,] b, double threshold)
        {
            for (int i = 0; i < a.GetLength(0); i++)
            {
                for (int j = 0; j < b.GetLength(1); j++)
                {
                    double x = a[i, j], y = b[i, j];

                    if (System.Math.Abs(x - y) > threshold || (Double.IsNaN(x) ^ Double.IsNaN(y)))
                        return false;
                }
            }
            return true;
        }

        /// <summary>
        ///   Compares two matrices for equality, considering an acceptance threshold.
        /// </summary>
        public static bool IsEqual(this float[,] a, float[,] b, float threshold)
        {
            for (int i = 0; i < a.GetLength(0); i++)
            {
                for (int j = 0; j < b.GetLength(1); j++)
                {
                    float x = a[i, j], y = b[i, j];

                    if (System.Math.Abs(x - y) > threshold || (Single.IsNaN(x) ^ Single.IsNaN(y)))
                        return false;
                }
            }
            return true;
        }


        /// <summary>
        ///   Compares two matrices for equality, considering an acceptance threshold.
        /// </summary>
        public static bool IsEqual(this double[][] a, double[][] b, double threshold)
        {
            for (int i = 0; i < a.Length; i++)
            {
                for (int j = 0; j < a[i].Length; j++)
                {
                    double x = a[i][j], y = b[i][j];

                    if (System.Math.Abs(x - y) > threshold || (Double.IsNaN(x) ^ Double.IsNaN(y)))
                    {
                        return false;
                    }
                }
            }
            return true;
        }

        /// <summary>
        ///   Compares two vectors for equality, considering an acceptance threshold.
        /// </summary>
        public static bool IsEqual(this double[] a, double[] b, double threshold)
        {
            for (int i = 0; i < a.Length; i++)
            {
                if (System.Math.Abs(a[i] - b[i]) > threshold)
                    return false;
            }
            return true;
        }

        /// <summary>
        ///   Compares each member of a vector for equality with a scalar value x.
        /// </summary>
        public static bool IsEqual(this double[] a, double x)
        {
            for (int i = 0; i < a.Length; i++)
            {
                if (a[i] != x)
                    return false;
            }
            return true;
        }

        /// <summary>
        ///   Compares each member of a matrix for equality with a scalar value x.
        /// </summary>
        public static bool IsEqual(this double[,] a, double x)
        {
            for (int i = 0; i < a.GetLength(0); i++)
            {
                for (int j = 0; j < a.GetLength(1); j++)
                {
                    if (a[i, j] != x)
                        return false;
                }
            }
            return true;
        }

        /// <summary>
        ///   Compares two matrices for equality.
        /// </summary>
        public static bool IsEqual<T>(this T[][] a, T[][] b)
        {
            if (a.Length != b.Length)
                return false;

            for (int i = 0; i < a.Length; i++)
            {
                if (a[i] == b[i])
                    continue;

                if (a[i] == null || b[i] == null)
                    return false;

                if (a[i].Length != b[i].Length)
                    return false;

                for (int j = 0; j < a[i].Length; j++)
                {
                    if (!a[i][j].Equals(b[i][j]))
                        return false;
                }
            }
            return true;
        }

        /// <summary>Compares two matrices for equality.</summary>
        public static bool IsEqual<T>(this T[,] a, T[,] b)
        {
            if (a.GetLength(0) != b.GetLength(0) ||
                a.GetLength(1) != b.GetLength(1))
                return false;

            int rows = a.GetLength(0);
            int cols = a.GetLength(1);

            for (int i = 0; i < rows; i++)
            {
                for (int j = 0; j < cols; j++)
                {
                    if (!a[i, j].Equals(b[i, j]))
                        return false;
                }
            }
            return true;
        }

        /// <summary>Compares two vectors for equality.</summary>
        public static bool IsEqual<T>(this T[] a, params T[] b)
        {
            if (a.Length != b.Length)
                return false;

            for (int i = 0; i < a.Length; i++)
            {
                if (!a[i].Equals(b[i]))
                    return false;
            }
            return true;
        }

        /// <summary>
        ///   This method should not be called. Use Matrix.IsEqual instead.
        /// </summary>
        public static new bool Equals(object value)
        {
            throw new NotSupportedException("Use Matrix.IsEqual instead.");
        }
        #endregion


        #region Matrix Construction

        /// <summary>
        ///   Creates a rows-by-cols matrix with uniformly distributed random data.
        /// </summary>
        public static double[,] Random(int rows, int cols)
        {
            return Random(rows, cols, 0, 1);
        }

        /// <summary>
        ///   Creates a rows-by-cols matrix with uniformly distributed random data.
        /// </summary>
        public static double[,] Random(int rows, int cols, int minValue, int maxValue)
        {
            double[,] r = new double[rows, cols];
            for (int i = 0; i < rows; i++)
                for (int j = 0; j < cols; j++)
                    r[i, j] = Accord.Math.Tools.Random.NextDouble() * (maxValue - minValue) + minValue;

            return r;
        }

        /// <summary>
        ///   Creates a magic square matrix.
        /// </summary>
        public static double[,] Magic(int size)
        {
            if (size < 3)
                throw new ArgumentException("The square size must be greater or equal to 3.", "size");

            double[,] m = new double[size, size];


            // First algorithm: Odd order
            if ((size % 2) == 1)
            {
                int a = (size + 1) / 2;
                int b = (size + 1);

                for (int j = 0; j < size; j++)
                    for (int i = 0; i < size; i++)
                        m[i, j] = size * ((i + j + a) % size) + ((i + 2 * j + b) % size) + 1;
            }

            // Second algorithm: Even order (double)
            else if ((size % 4) == 0)
            {
                for (int j = 0; j < size; j++)
                    for (int i = 0; i < size; i++)
                        if (((i + 1) / 2) % 2 == ((j + 1) / 2) % 2)
                            m[i, j] = size * size - size * i - j;
                        else
                            m[i, j] = size * i + j + 1;
            }

            // Third algorithm: Even order (single)
            else
            {
                int n = size / 2;
                int p = (size - 2) / 4;
                double t;

                var a = Matrix.Magic(n);

                for (int j = 0; j < n; j++)
                {
                    for (int i = 0; i < n; i++)
                    {
                        double e = a[i, j];
                        m[i, j] = e;
                        m[i, j + n] = e + 2 * n * n;
                        m[i + n, j] = e + 3 * n * n;
                        m[i + n, j + n] = e + n * n;
                    }
                }

                for (int i = 0; i < n; i++)
                {
                    // Swap M[i,j] and M[i+n,j]
                    for (int j = 0; j < p; j++)
                    {
                        t = m[i, j];
                        m[i, j] = m[i + n, j];
                        m[i + n, j] = t;
                    }
                    for (int j = size - p + 1; j < size; j++)
                    {
                        t = m[i, j];
                        m[i, j] = m[i + n, j];
                        m[i + n, j] = t;
                    }
                }

                // Continue swaping in the boundary
                t = m[p, 0];
                m[p, 0] = m[p + n, 0];
                m[p + n, 0] = t;

                t = m[p, p];
                m[p, p] = m[p + n, p];
                m[p + n, p] = t;
            }

            return m; // return the magic square.
        }

        /// <summary>
        ///   Gets the diagonal vector from a matrix.
        /// </summary>
        /// <param name="m">A matrix.</param>
        /// <returns>The diagonal vector from matrix m.</returns>
        public static T[] Diagonal<T>(this T[,] m)
        {
            T[] r = new T[m.GetLength(0)];

            for (int i = 0; i < r.Length; i++)
                r[i] = m[i, i];

            return r;
        }

        /// <summary>
        ///   Returns a square diagonal matrix of the given size.
        /// </summary>
        public static T[,] Diagonal<T>(int size, T value)
        {
            T[,] m = new T[size, size];

            for (int i = 0; i < size; i++)
                m[i, i] = value;

            return m;
        }

        /// <summary>
        ///   Returns a matrix of the given size with value on its diagonal.
        /// </summary>
        public static T[,] Diagonal<T>(int rows, int cols, T value)
        {
            T[,] m = new T[rows, cols];

            for (int i = 0; i < rows; i++)
                m[i, i] = value;

            return m;
        }

        /// <summary>
        ///   Return a square matrix with a vector of values on its diagonal.
        /// </summary>
        public static T[,] Diagonal<T>(T[] values)
        {
            T[,] m = new T[values.Length, values.Length];

            for (int i = 0; i < values.Length; i++)
                m[i, i] = values[i];

            return m;
        }

        /// <summary>
        ///   Return a square matrix with a vector of values on its diagonal.
        /// </summary>
        public static T[,] Diagonal<T>(int size, T[] values)
        {
            return Diagonal(size, size, values);
        }

        /// <summary>
        ///   Returns a matrix with a vector of values on its diagonal.
        /// </summary>
        public static T[,] Diagonal<T>(int rows, int cols, T[] values)
        {
            T[,] m = new T[rows, cols];

            for (int i = 0; i < values.Length; i++)
                m[i, i] = values[i];

            return m;
        }

        /// <summary>
        ///   Returns a matrix with all elements set to a given value.
        /// </summary>
        public static T[,] Create<T>(int rows, int cols, T value)
        {
            T[,] m = new T[rows, cols];

            for (int i = 0; i < rows; i++)
                for (int j = 0; j < cols; j++)
                    m[i, j] = value;

            return m;
        }

        /// <summary>
        ///   Returns a matrix with all elements set to a given value.
        /// </summary>
        public static T[,] Create<T>(int size, T value)
        {
            return Create(size, size, value);
        }

        /// <summary>
        ///   Expands a data vector given in summary form.
        /// </summary>
        /// <param name="data">A base vector.</param>
        /// <param name="count">An array containing by how much each line should be replicated.</param>
        /// <returns></returns>
        public static T[] Expand<T>(T[] data, int[] count)
        {
            var expansion = new List<T>();
            for (int i = 0; i < count.Length; i++)
                for (int j = 0; j < count[i]; j++)
                    expansion.Add(data[i]);

            return expansion.ToArray();
        }

        /// <summary>
        ///   Expands a data matrix given in summary form.
        /// </summary>
        /// <param name="data">A base matrix.</param>
        /// <param name="count">An array containing by how much each line should be replicated.</param>
        /// <returns></returns>
        public static T[][] Expand<T>(T[][] data, int[] count)
        {
            var expansion = new List<T[]>();
            for (int i = 0; i < count.Length; i++)
                for (int j = 0; j < count[i]; j++)
                    expansion.Add(data[i]);

            return expansion.ToArray();
        }

        /// <summary>
        ///   Returns the Identity matrix of the given size.
        /// </summary>
        public static double[,] Identity(int size)
        {
            return Diagonal(size, 1.0);
        }

        /// <summary>
        ///   Creates a centering matrix of size n x n in the form (I - 1n)
        ///   where 1n is a matrix with all entries 1/n.
        /// </summary>
        public static double[,] Centering(int size)
        {
            double[,] r = Matrix.Create(size, -1.0 / size);

            for (int i = 0; i < size; i++)
                r[i, i] = 1.0 - 1.0 / size;

            return r;
        }

        /// <summary>
        ///   Creates a matrix with a single row vector.
        /// </summary>
        public static T[,] RowVector<T>(params T[] values)
        {
            T[,] r = new T[1, values.Length];

            for (int i = 0; i < values.Length; i++)
                r[0, i] = values[i];

            return r;
        }

        /// <summary>
        ///   Creates a matrix with a single column vector.
        /// </summary>
        public static T[,] ColumnVector<T>(T[] values)
        {
            T[,] r = new T[values.Length, 1];

            for (int i = 0; i < values.Length; i++)
                r[i, 0] = values[i];

            return r;
        }

        /// <summary>
        ///   Creates a index vector.
        /// </summary>
        public static int[] Indexes(int from, int to)
        {
            int[] r = new int[to - from];
            for (int i = 0; i < r.Length; i++)
                r[i] = from++;
            return r;
        }

        /// <summary>
        ///   Creates an interval vector.
        /// </summary>
        public static int[] Interval(int from, int to)
        {
            int[] r = new int[to - from + 1];
            for (int i = 0; i < r.Length; i++)
                r[i] = from++;
            return r;
        }

        /// <summary>
        ///   Creates an interval vector.
        /// </summary>
        public static double[] Interval(double from, double to, double stepSize)
        {
            double range = to - from;
            int steps = (int)Math.Ceiling(range / stepSize) + 1;

            double[] r = new double[steps];
            for (int i = 0; i < r.Length; i++)
                r[i] = from + i * stepSize;

            return r;
        }

        /// <summary>
        ///   Creates an interval vector.
        /// </summary>
        public static double[] Interval(double from, double to, int steps)
        {
            double range = to - from;
            double stepSize = range / steps;

            double[] r = new double[steps + 1];
            for (int i = 0; i < r.Length; i++)
                r[i] = i * stepSize;

            return r;
        }


        /// <summary>
        ///   Combines two vectors horizontally.
        /// </summary>
        public static T[] Combine<T>(this T[] a1, T[] a2)
        {
            T[] r = new T[a1.Length + a2.Length];
            for (int i = 0; i < a1.Length; i++)
                r[i] = a1[i];
            for (int i = 0; i < a2.Length; i++)
                r[i + a1.Length] = a2[i];

            return r;
        }

        /// <summary>
        ///   Combines a vector and a element horizontally.
        /// </summary>
        public static T[] Combine<T>(this T[] a1, T a2)
        {
            T[] r = new T[a1.Length + 1];
            for (int i = 0; i < a1.Length; i++)
                r[i] = a1[i];

            r[a1.Length] = a2;

            return r;
        }

        /// <summary>
        ///   Combine vectors horizontally.
        /// </summary>
        public static T[] Combine<T>(params T[][] vectors)
        {
            int size = 0;
            for (int i = 0; i < vectors.Length; i++)
                size += vectors[i].Length;

            T[] r = new T[size];

            int c = 0;
            for (int i = 0; i < vectors.Length; i++)
                for (int j = 0; j < vectors[i].Length; j++)
                    r[c++] = vectors[i][j];

            return r;
        }

        /// <summary>
        ///   Combines matrices vertically.
        /// </summary>
        public static T[,] Combine<T>(params T[][,] matrices)
        {
            int rows = 0;
            int cols = 0;

            for (int i = 0; i < matrices.Length; i++)
            {
                rows += matrices[i].GetLength(0);
                if (matrices[i].GetLength(1) > cols)
                    cols = matrices[i].GetLength(1);
            }

            T[,] r = new T[rows, cols];

            int c = 0;
            for (int i = 0; i < matrices.Length; i++)
            {
                for (int j = 0; j < matrices[i].GetLength(0); j++)
                {
                    for (int k = 0; k < matrices[i].GetLength(1); k++)
                        r[c, k] = matrices[i][j, k];
                    c++;
                }
            }

            return r;
        }

        /// <summary>
        ///   Combines matrices vertically.
        /// </summary>
        public static T[][] Combine<T>(params T[][][] matrices)
        {
            int rows = 0;
            int cols = 0;

            for (int i = 0; i < matrices.Length; i++)
            {
                rows += matrices[i].Length;
                if (matrices[i][0].Length > cols)
                    cols = matrices[i][0].Length;
            }

            T[][] r = new T[rows][];
            for (int i = 0; i < rows; i++)
                r[i] = new T[cols];

            int c = 0;
            for (int i = 0; i < matrices.Length; i++)
            {
                for (int j = 0; j < matrices[i].Length; j++)
                {
                    for (int k = 0; k < matrices[i][0].Length; k++)
                        r[c][k] = matrices[i][j][k];
                    c++;
                }
            }

            return r;
        }
        #endregion


        #region Subsection and elements selection

        /// <summary>Returns a sub matrix extracted from the current matrix.</summary>
        /// <param name="data">The matrix to return the submatrix from.</param>
        /// <param name="startRow">Start row index</param>
        /// <param name="endRow">End row index</param>
        /// <param name="startColumn">Start column index</param>
        /// <param name="endColumn">End column index</param>
        /// <remarks>
        ///   Routine adapted from Lutz Roeder's Mapack for .NET, September 2000.
        /// </remarks>
        public static T[,] Submatrix<T>(this T[,] data, int startRow, int endRow, int startColumn, int endColumn)
        {
            int rows = data.GetLength(0);
            int cols = data.GetLength(1);

            if ((startRow > endRow) || (startColumn > endColumn) || (startRow < 0) ||
                (startRow >= rows) || (endRow < 0) || (endRow >= rows) ||
                (startColumn < 0) || (startColumn >= cols) || (endColumn < 0) ||
                (endColumn >= cols))
            {
                throw new ArgumentException("Argument out of range.");
            }

            T[,] X = new T[endRow - startRow + 1, endColumn - startColumn + 1];
            for (int i = startRow; i <= endRow; i++)
            {
                for (int j = startColumn; j <= endColumn; j++)
                {
                    X[i - startRow, j - startColumn] = data[i, j];
                }
            }

            return X;
        }

        /// <summary>Returns a sub matrix extracted from the current matrix.</summary>
        /// <param name="data">The matrix to return the submatrix from.</param>
        /// <param name="rowIndexes">Array of row indices</param>
        /// <param name="columnIndexes">Array of column indices</param>
        /// <remarks>
        ///   Routine adapted from Lutz Roeder's Mapack for .NET, September 2000.
        /// </remarks>
        public static T[,] Submatrix<T>(this T[,] data, int[] rowIndexes, int[] columnIndexes)
        {
            T[,] X = new T[rowIndexes.Length, columnIndexes.Length];

            for (int i = 0; i < rowIndexes.Length; i++)
            {
                for (int j = 0; j < columnIndexes.Length; j++)
                {
                    if ((rowIndexes[i] < 0) || (rowIndexes[i] >= data.GetLength(0)) ||
                        (columnIndexes[j] < 0) || (columnIndexes[j] >= data.GetLength(1)))
                    {
                        throw new ArgumentException("Argument out of range.");
                    }

                    X[i, j] = data[rowIndexes[i], columnIndexes[j]];
                }
            }

            return X;
        }

        /// <summary>Returns a sub matrix extracted from the current matrix.</summary>
        /// <param name="data">The matrix to return the submatrix from.</param>
        /// <param name="rowIndexes">Array of row indices</param>
        /// <remarks>
        ///   Routine adapted from Lutz Roeder's Mapack for .NET, September 2000.
        /// </remarks>
        public static T[,] Submatrix<T>(this T[,] data, int[] rowIndexes)
        {
            T[,] X = new T[rowIndexes.Length, data.GetLength(1)];

            for (int i = 0; i < rowIndexes.Length; i++)
            {
                for (int j = 0; j < data.GetLength(1); j++)
                {
                    if ((rowIndexes[i] < 0) || (rowIndexes[i] >= data.GetLength(0)))
                    {
                        throw new ArgumentException("Argument out of range.");
                    }

                    X[i, j] = data[rowIndexes[i], j];
                }
            }

            return X;
        }

        /// <summary>Returns a subvector extracted from the current vector.</summary>
        /// <param name="data">The vector to return the subvector from.</param>
        /// <param name="indexes">Array of indices.</param>
        /// <remarks>
        ///   Routine adapted from Lutz Roeder's Mapack for .NET, September 2000.
        /// </remarks>
        public static T[] Submatrix<T>(this T[] data, int[] indexes)
        {
            T[] X = new T[indexes.Length];

            for (int i = 0; i < indexes.Length; i++)
            {
                X[i] = data[indexes[i]];
            }

            return X;
        }

        /// <summary>Returns a sub matrix extracted from the current matrix.</summary>
        /// <param name="data">The vector to return the subvector from.</param>
        /// <param name="i0">Starting index.</param>
        /// <param name="i1">End index.</param>
        /// <remarks>
        ///   Routine adapted from Lutz Roeder's Mapack for .NET, September 2000.
        /// </remarks>
        public static T[] Submatrix<T>(this T[] data, int i0, int i1)
        {
            T[] X = new T[i1 - i0 + 1];

            for (int i = i0; i <= i1; i++)
            {
                X[i - i0] = data[i];
            }

            return X;
        }

        /// <summary>Returns a sub matrix extracted from the current matrix.</summary>
        /// <remarks>
        ///   Routine adapted from Lutz Roeder's Mapack for .NET, September 2000.
        /// </remarks>
        public static T[] Submatrix<T>(this T[] data, int first)
        {
            if (first < 1 || first > data.Length)
                throw new ArgumentOutOfRangeException("first");

            return Submatrix<T>(data, 0, first - 1);
        }

        /// <summary>Returns a sub matrix extracted from the current matrix.</summary>
        /// <param name="data">The matrix to return the submatrix from.</param>
        /// <param name="i0">Starting row index</param>
        /// <param name="i1">End row index</param>
        /// <param name="c">Array of column indices</param>
        /// <remarks>
        ///   Routine adapted from Lutz Roeder's Mapack for .NET, September 2000.
        /// </remarks>
        public static T[,] Submatrix<T>(this T[,] data, int i0, int i1, int[] c)
        {
            if ((i0 > i1) || (i0 < 0) || (i0 >= data.GetLength(0))
                || (i1 < 0) || (i1 >= data.GetLength(0)))
            {
                throw new ArgumentException("Argument out of range.");
            }

            T[,] X = new T[i1 - i0 + 1, c.Length];

            for (int i = i0; i <= i1; i++)
            {
                for (int j = 0; j < c.Length; j++)
                {
                    if ((c[j] < 0) || (c[j] >= data.GetLength(1)))
                    {
                        throw new ArgumentException("Argument out of range.");
                    }

                    X[i - i0, j] = data[i, c[j]];
                }
            }

            return X;
        }

        /// <summary>Returns a sub matrix extracted from the current matrix.</summary>
        /// <param name="data">The matrix to return the submatrix from.</param>
        /// <param name="r">Array of row indices</param>
        /// <param name="j0">Start column index</param>
        /// <param name="j1">End column index</param>
        /// <remarks>
        ///   Routine adapted from Lutz Roeder's Mapack for .NET, September 2000.
        /// </remarks>
        public static T[,] Submatrix<T>(this T[,] data, int[] r, int j0, int j1)
        {
            if ((j0 > j1) || (j0 < 0) || (j0 >= data.GetLength(1)) || (j1 < 0)
                || (j1 >= data.GetLength(1)))
            {
                throw new ArgumentException("Argument out of range.");
            }

            T[,] X = new T[r.Length, j1 - j0 + 1];

            for (int i = 0; i < r.Length; i++)
            {
                for (int j = j0; j <= j1; j++)
                {
                    if ((r[i] < 0) || (r[i] >= data.GetLength(0)))
                    {
                        throw new ArgumentException("Argument out of range.");
                    }

                    X[i, j - j0] = data[r[i], j];
                }
            }

            return X;
        }

        /// <summary>Returns a sub matrix extracted from the current matrix.</summary>
        /// <param name="data">The matrix to return the submatrix from.</param>
        /// <param name="i0">Starting row index</param>
        /// <param name="i1">End row index</param>
        /// <param name="c">Array of column indices</param>
        /// <remarks>
        ///   Routine adapted from Lutz Roeder's Mapack for .NET, September 2000.
        /// </remarks>
        public static T[][] Submatrix<T>(this T[][] data, int i0, int i1, int[] c)
        {
            if ((i0 > i1) || (i0 < 0) || (i0 >= data.Length)
                || (i1 < 0) || (i1 >= data.Length))
            {
                throw new ArgumentException("Argument out of range");
            }

            T[][] X = new T[i1 - i0 + 1][];

            for (int i = i0; i <= i1; i++)
            {
                X[i] = new T[c.Length];

                for (int j = 0; j < c.Length; j++)
                {
                    if ((c[j] < 0) || (c[j] >= data[i].Length))
                    {
                        throw new ArgumentException("Argument out of range.");
                    }

                    X[i - i0][j] = data[i][c[j]];
                }
            }

            return X;
        }

        /// <summary>
        ///   Returns a new matrix without one of its columns.
        /// </summary>
        public static T[][] RemoveColumn<T>(this T[][] m, int index)
        {
            T[][] X = new T[m.Length][];

            for (int i = 0; i < m.Length; i++)
            {
                X[i] = new T[m[i].Length - 1];
                for (int j = 0; j < index; j++)
                {
                    X[i][j] = m[i][j];
                }
                for (int j = index + 1; j < m[i].Length; j++)
                {
                    X[i][j - 1] = m[i][j];
                }
            }
            return X;
        }

        /// <summary>
        ///   Returns a new matrix with a given column vector inserted at a given index.
        /// </summary>
        public static T[,] InsertColumn<T>(this T[,] m, T[] column, int index)
        {
            int rows = m.GetLength(0);
            int cols = m.GetLength(1);

            T[,] X = new T[rows, cols + 1];

            for (int i = 0; i < rows; i++)
            {
                // Copy original matrix
                for (int j = 0; j < index; j++)
                {
                    X[i, j] = m[i, j];
                }
                for (int j = index; j < cols; j++)
                {
                    X[i, j + 1] = m[i, j];
                }

                // Copy additional column
                X[i, index] = column[i];
            }

            return X;
        }

        /// <summary>
        ///   Removes an element from a vector.
        /// </summary>
        public static T[] RemoveAt<T>(this T[] array, int index)
        {
            T[] r = new T[array.Length - 1];
            for (int i = 0; i < index; i++)
                r[i] = array[i];
            for (int i = index; i < r.Length; i++)
                r[i] = array[i + 1];

            return r;
        }



        /// <summary>
        ///   Gets a column vector from a matrix.
        /// </summary>
        public static T[] GetColumn<T>(this T[,] m, int index)
        {
            T[] column = new T[m.GetLength(0)];

            for (int i = 0; i < column.Length; i++)
                column[i] = m[i, index];

            return column;
        }

        /// <summary>
        ///   Gets a column vector from a matrix.
        /// </summary>
        public static T[] GetColumn<T>(this T[][] m, int index)
        {
            T[] column = new T[m.Length];

            for (int i = 0; i < column.Length; i++)
                column[i] = m[i][index];

            return column;
        }

        /// <summary>
        ///   Stores a column vector into the given column position of the matrix.
        /// </summary>
        public static T[,] SetColumn<T>(this T[,] m, int index, T[] column)
        {
            for (int i = 0; i < column.Length; i++)
                m[i, index] = column[i];

            return m;
        }

        /// <summary>
        ///   Gets a row vector from a matrix.
        /// </summary>
        public static T[] GetRow<T>(this T[,] m, int index)
        {
            T[] row = new T[m.GetLength(1)];

            for (int i = 0; i < row.Length; i++)
                row[i] = m[index, i];

            return row;
        }

        /// <summary>
        ///   Stores a row vector into the given row position of the matrix.
        /// </summary>
        public static T[,] SetRow<T>(this T[,] m, int index, T[] row)
        {
            for (int i = 0; i < row.Length; i++)
                m[index, i] = row[i];

            return m;
        }


        /// <summary>
        ///   Gets the indices of all elements matching a certain criteria.
        /// </summary>
        /// <typeparam name="T">The type of the array.</typeparam>
        /// <param name="data">The array to search inside.</param>
        /// <param name="func">The search criteria.</param>
        public static int[] Find<T>(this T[] data, Func<T, bool> func)
        {
            return Find(data, func, false);
        }

        /// <summary>
        ///   Gets the indices of all elements matching a certain criteria.
        /// </summary>
        /// <typeparam name="T">The type of the array.</typeparam>
        /// <param name="data">The array to search inside.</param>
        /// <param name="func">The search criteria.</param>
        /// <param name="firstOnly">
        ///    Set to true to stop when the first element is
        ///    found, set to false to get all elements.
        /// </param>
        public static int[] Find<T>(this T[] data, Func<T, bool> func, bool firstOnly)
        {
            List<int> idx = new List<int>();
            for (int i = 0; i < data.Length; i++)
            {
                if (func(data[i]))
                {
                    if (firstOnly)
                        return new int[] { i };
                    else idx.Add(i);
                }
            }
            return idx.ToArray();
        }

        /// <summary>
        ///   Gets the indices of all elements matching a certain criteria.
        /// </summary>
        /// <typeparam name="T">The type of the array.</typeparam>
        /// <param name="data">The array to search inside.</param>
        /// <param name="func">The search criteria.</param>
        public static int[][] Find<T>(this T[,] data, Func<T, bool> func)
        {
            return Find(data, func, false);
        }

        /// <summary>
        ///   Gets the indices of all elements matching a certain criteria.
        /// </summary>
        /// <typeparam name="T">The type of the array.</typeparam>
        /// <param name="data">The array to search inside.</param>
        /// <param name="func">The search criteria.</param>
        /// <param name="firstOnly">
        ///    Set to true to stop when the first element is
        ///    found, set to false to get all elements.
        /// </param>
        public static int[][] Find<T>(this T[,] data, Func<T, bool> func, bool firstOnly)
        {
            List<int[]> idx = new List<int[]>();
            for (int i = 0; i < data.GetLength(0); i++)
            {
                for (int j = 0; j < data.GetLength(1); j++)
                {
                    if (func(data[i, j]))
                    {
                        if (firstOnly)
                            return new int[][] { new int[] { i, j } };
                        else idx.Add(new int[] { i, j });
                    }
                }
            }
            return idx.ToArray();
        }

        /// <summary>
        ///   Applies a function to every element of the array.
        /// </summary>
        public static TResult[] Apply<TData, TResult>(this TData[] data, Func<TData, TResult> func)
        {
            var r = new TResult[data.Length];
            for (int i = 0; i < data.Length; i++)
                r[i] = func(data[i]);
            return r;
        }

        /// <summary>
        ///   Applies a function to every element of a matrix.
        /// </summary>
        public static TResult[,] Apply<TData, TResult>(this TData[,] data, Func<TData, TResult> func)
        {
            int rows = data.GetLength(0);
            int cols = data.GetLength(1);

            var r = new TResult[rows, cols];
            for (int i = 0; i < rows; i++)
                for (int j = 0; j < cols; j++)
                    r[i, j] = func(data[i, j]);

            return r;
        }


        /// <summary>
        ///   Applies a function to every element of the array.
        /// </summary>
        public static void ApplyInPlace<TData>(this TData[] data, Func<TData, TData> func)
        {
            for (int i = 0; i < data.Length; i++)
                data[i] = func(data[i]);
        }

        /// <summary>
        ///   Applies a function to every element of the array.
        /// </summary>
        public static void ApplyInPlace<TData>(this TData[] data, Func<TData, int, TData> func)
        {
            for (int i = 0; i < data.Length; i++)
                data[i] = func(data[i], i);
        }

        /// <summary>
        ///   Sorts the columns of a matrix by sorting keys.
        /// </summary>
        /// <param name="keys">The key value for each column.</param>
        /// <param name="values">The matrix to be sorted.</param>
        /// <param name="comparer">The comparer to use.</param>
        public static TValue[,] Sort<TKey, TValue>(TKey[] keys, TValue[,] values, IComparer<TKey> comparer)
        {
            int[] indices = new int[keys.Length];
            for (int i = 0; i < keys.Length; i++) indices[i] = i;

            Array.Sort<TKey, int>(keys, indices, comparer);

            return values.Submatrix(0, values.GetLength(0) - 1, indices);
        }

        /// <summary>
        ///   Splits a given vector into a smaller vectors of the given size.
        /// </summary>
        /// <param name="vector">The vector to be splitted.</param>
        /// <param name="size">The size of the resulting vectors.</param>
        /// <returns>An array of vectors containing the subdivisions of the given vector.</returns>
        public static T[][] Split<T>(this T[] vector, int size)
        {
            int n = vector.Length / size;
            T[][] r = new T[n][];
            for (int i = 0; i < n; i++)
            {
                T[] ri = r[i] = new T[size];
                for (int j = 0; j < size; j++)
                    ri[j] = vector[j * n + i];
            }
            return r;
        }
        #endregion


        #region Elementwise operations
        /// <summary>
        ///   Elementwise absolute value.
        /// </summary>
        public static int[] Abs(this int[] value)
        {
            int[] r = new int[value.Length];
            for (int i = 0; i < value.Length; i++)
                r[i] = System.Math.Abs(value[i]);
            return r;
        }

        /// <summary>
        ///   Elementwise absolute value.
        /// </summary>
        public static double[] Abs(this double[] value)
        {
            double[] r = new double[value.Length];
            for (int i = 0; i < value.Length; i++)
                r[i] = System.Math.Abs(value[i]);
            return r;
        }

        /// <summary>
        ///   Elementwise absolute value.
        /// </summary>
        public static double[,] Abs(this double[,] value)
        {
            int rows = value.GetLength(0);
            int cols = value.GetLength(1);

            double[,] r = new double[rows, cols];
            for (int i = 0; i < rows; i++)
                for (int j = 0; j < cols; j++)
                    r[i, j] = System.Math.Abs(value[i, j]);
            return r;
        }

        /// <summary>
        ///   Elementwise absolute value.
        /// </summary>
        public static int[,] Abs(this int[,] value)
        {
            int rows = value.GetLength(0);
            int cols = value.GetLength(1);

            int[,] r = new int[rows, cols];
            for (int i = 0; i < rows; i++)
                for (int j = 0; j < cols; j++)
                    r[i, j] = System.Math.Abs(value[i, j]);
            return r;
        }


        /// <summary>
        ///   Elementwise Square root.
        /// </summary>
        public static double[] Sqrt(this double[] value)
        {
            double[] r = new double[value.Length];
            for (int i = 0; i < value.Length; i++)
                r[i] = System.Math.Sqrt(value[i]);
            return r;
        }

        /// <summary>
        ///   Elementwise Square root.
        /// </summary>
        public static double[,] Sqrt(this double[,] value)
        {
            int rows = value.GetLength(0);
            int cols = value.GetLength(1);

            double[,] r = new double[rows, cols];
            for (int i = 0; i < rows; i++)
                for (int j = 0; j < cols; j++)
                    r[i, j] = System.Math.Sqrt(value[i, j]);
            return r;
        }


        /// <summary>
        ///   Elementwise power operation.
        /// </summary>
        /// <param name="x">A matrix.</param>
        /// <param name="y">A power.</param>
        /// <returns>Returns x elevated to the power of y.</returns>
        public static double[,] ElementwisePower(this double[,] x, double y)
        {
         //   double[,] r = new double[x.GetLength(0), x.GetLength(1)];

            for (int i = 0; i < x.GetLength(0); i++)
                for (int j = 0; j < x.GetLength(1); j++)
                    x[i, j] = System.Math.Pow(x[i, j], y);

            return x;
        }

        /// <summary>
        ///   Elementwise power operation.
        /// </summary>
        /// <param name="x">A matrix.</param>
        /// <param name="y">A power.</param>
        /// <returns>Returns x elevated to the power of y.</returns>
        public static double[] ElementwisePower(this double[] x, double y)
        {
            double[] r = new double[x.Length];

            for (int i = 0; i < r.Length; i++)
                r[i] = System.Math.Pow(x[i], y);

            return r;
        }


        /// <summary>
        ///   Elementwise divide operation.
        /// </summary>
        public static double[] ElementwiseDivide(this double[] a, double[] b)
        {
            double[] r = new double[a.Length];

            for (int i = 0; i < a.Length; i++)
                r[i] = a[i] / b[i];

            return r;
        }

        /// <summary>
        ///   Elementwise divide operation.
        /// </summary>
        public static double[,] ElementwiseDivide(this double[,] a, double[,] b)
        {
            int rows = a.GetLength(0);
            int cols = b.GetLength(1);

            double[,] r = new double[rows, cols];

            for (int i = 0; i < rows; i++)
                for (int j = 0; j < cols; j++)
                    r[i, j] = a[i, j] / b[i, j];

            return r;
        }

        /// <summary>
        ///   Elementwise division.
        /// </summary>
        public static double[,] ElementwiseDivide(this double[,] a, double[] b, int dimension)
        {
            int rows = a.GetLength(0);
            int cols = a.GetLength(1);

            double[,] r = new double[rows, cols];

            if (dimension == 1)
            {
                if (cols != b.Length) throw new ArgumentException(
                        "Length of B should equal the number of columns in A", "b");

                for (int i = 0; i < rows; i++)
                    for (int j = 0; j < cols; j++)
                        r[i, j] = a[i, j] / b[j];
            }
            else
            {
                if (rows != b.Length) throw new ArgumentException(
                        "Length of B should equal the number of rows in A", "b");

                for (int j = 0; j < cols; j++)
                    for (int i = 0; i < rows; i++)
                        r[i, j] = a[i, j] / b[i];
            }
            return r;
        }


        /// <summary>
        ///   Elementwise multiply operation.
        /// </summary>
        public static double[] ElementwiseMultiply(this double[] a, double[] b)
        {
            double[] r = new double[a.Length];

            for (int i = 0; i < a.Length; i++)
                r[i] = a[i] * b[i];

            return r;
        }

        /// <summary>
        ///   Elementwise multiply operation.
        /// </summary>
        public static double[,] ElementwiseMultiply(this double[,] a, double[,] b)
        {
            if (a.GetLength(0) != b.GetLength(0) || a.GetLength(1) != b.GetLength(1))
                throw new ArgumentException("Matrix dimensions must agree.", "b");

            int rows = a.GetLength(0);
            int cols = a.GetLength(1);

            double[,] r = new double[rows, cols];

            for (int i = 0; i < rows; i++)
                for (int j = 0; j < cols; j++)
                    r[i, j] = a[i, j] * b[i, j];

            return r;
        }

        /// <summary>
        ///   Elementwise multiply operation.
        /// </summary>
        public static int[] ElementwiseMultiply(this int[] a, int[] b)
        {
            int[] r = new int[a.Length];

            for (int i = 0; i < a.Length; i++)
                r[i] = a[i] * b[i];

            return r;
        }

        /// <summary>
        ///   Elementwise multiplication.
        /// </summary>
        public static int[,] ElementwiseMultiply(this int[,] a, int[,] b)
        {
            if (a.GetLength(0) != b.GetLength(0) || a.GetLength(1) != b.GetLength(1))
                throw new ArgumentException("Matrix dimensions must agree.", "b");

            int rows = a.GetLength(0);
            int cols = a.GetLength(1);

            int[,] r = new int[rows, cols];

            for (int i = 0; i < rows; i++)
                for (int j = 0; j < cols; j++)
                    r[i, j] = a[i, j] * b[i, j];

            return r;
        }

        /// <summary>
        ///   Elementwise multiplication.
        /// </summary>
        public static double[,] ElementwiseMultiply(this double[,] a, double[] b, int dimension)
        {
            int rows = a.GetLength(0);
            int cols = a.GetLength(1);

            double[,] r = new double[rows, cols];

            if (dimension == 1)
            {
                if (cols != b.Length) throw new ArgumentException(
                        "Length of B should equal the number of columns in A", "b");

                for (int i = 0; i < rows; i++)
                    for (int j = 0; j < cols; j++)
                        r[i, j] = a[i, j] * b[j];
            }
            else
            {
                if (rows != b.Length) throw new ArgumentException(
                        "Length of B should equal the number of rows in A", "b");

                for (int j = 0; j < cols; j++)
                    for (int i = 0; i < rows; i++)
                        r[i, j] = a[i, j] * b[i];
            }

            return r;
        }

        #endregion
        #region myFunc

        public static double[,] addition(this double[,] right,double[,] left)
        {
            int rows = right.GetLength(0);
            int cols = right.GetLength(1);

         //   double[,] add = new double[rows, cols];

            for (int i = 0; i < rows; i++)
                for (int j = 0; j < cols; j++)
                    right[i,j] = right[i, j]+left[i,j];
                
            return right;
         }

        public static double[,] substriction(this double[,] right, double[,] left)
        {
            int rows = right.GetLength(0);
            int cols = right.GetLength(1);

            //   double[,] add = new double[rows, cols];

            for (int i = 0; i < rows; i++)
                for (int j = 0; j < cols; j++)
                    right[i, j] = right[i, j] - left[i, j];

            return right;
        }

        public static double[,] Transpose(this double[,] a)
        {
            int rows = a.GetLength(0);
            int cols = a.GetLength(1);

            double[,] transMat = new double[cols, rows];

            for (int i = 0; i < rows; i++)
                for (int j = 0; j < cols; j++)
                {
                    if (i>rows||j>cols) throw new IndexOutOfRangeException("Length of B should equal the number of rows in A, b");
                    transMat[j, i] = a[i, j];
                }
            return transMat; 
        }

        public static double[,] MultiplyMatrix(this double[,]right,double [,]left)
        {
            if (right.GetLength(1) == left.GetLength(0))
            {
                double[,] c = new double[right.GetLength(0), left.GetLength(1)];
                for (int i = 0; i < c.GetLength(0); i++)
                {
                    for (int j = 0; j < c.GetLength(1); j++)
                    {
                        c[i, j] = 0;
                        for (int k = 0; k < right.GetLength(1); k++) // OR k<b.GetLength(0)
                        {
                            if (Double.IsNaN(left[k,j])) 
                                left[k, j] = 0.0;  
                          c[i, j] = c[i, j] + right[i, k] * left[k, j];
                        }
                    }
                }
                return c;
            }
            else
            {
                Console.WriteLine("\n Number of columns in Matrix1 is not equal to Number of rows in Matrix2.");
                Console.WriteLine("\n Therefore Multiplication of Matrix1 with Matrix2 is not possible");
                return null;
            }
        }

        public static double[,] devideMatrixByNumber(this double[,] up, double down)
        {
            int rows = up.GetLength(0);
            int cols = up.GetLength(1);

            double[,] div = new double[rows, cols];

            for (int i = 0; i < rows; i++)
                for (int j = 0; j < cols; j++)
                    div[j, i] = up[i, j] /down;

            return div;
        }

        public static double[,] multiplyMatrixByNumber(this double[,] right, double left)
        {
            int rows = right.GetLength(0);
            int cols = right.GetLength(1);

            double[,] mul = new double[rows, cols];

            for (int i = 0; i < rows; i++)
                for (int j = 0; j < cols; j++)
                    mul[j, i] = right[i, j] * left;

            return mul;
        }

        public static double Det(this double[,] Mat)
        {
            int S, k, k1, i, j;
            double[,] DArray = (double[,])Mat.Clone();
            double save, ArrayK, tmpDet;
            int Rows = Mat.GetLength(0), Cols = Mat.GetLength(1);

            if (Rows != Cols) throw new MatrixNotSquare();

            S = Rows;
            tmpDet = 1;

            for (k = 0; k < S; k++)
            {
                if (DArray[k, k] == 0)
                {
                    j = k;
                    while ((j < S) && (DArray[k, j] == 0)) j = j + 1;
                    if (DArray[k, j-1] == 0) return 0;
                    else
                    {
                        for (i = k; i < S; i++)
                        {
                            save = DArray[i, j];
                            DArray[i, j] = DArray[i, k];
                            DArray[i, k] = save;
                        }
                    }
                    tmpDet = -tmpDet;
                }
                ArrayK = DArray[k, k];
                tmpDet = tmpDet * ArrayK;
                if (k < S)
                {
                    k1 = k + 1;
                    for (i = k1; i < S; i++)
                    {
                        for (j = k1; j <S; j++)
                            DArray[i, j] = DArray[i, j] - DArray[i, k] * (DArray[k, j] / ArrayK);
                    }
                }
            }
            return tmpDet;
        }

        public static double[,] inverse(this double[,] Mat)
        {
            double det = Mat.Det();
            double[,] AI, Mat1 = (double[,])Mat.Clone(); ;
            double AIN, AF;
            int Rows = Mat.GetLength(0), Cols = Mat.GetLength(1);
            int LL, LLM, L1, L2, LC, LCA, LCB;
            det = 1 / det;
            if (Mat.GetLength(0) != Mat.GetLength(1)) throw new MatrixNotSquare();
            if (det == 0) throw new MatrixDeterminentZero();

            LL = Rows;
            LLM = Cols;
            AI = new double[LL, LL];

            for (L2 = 0; L2 < LL; L2++)
            {
                for (L1 = 0; L1 < LL; L1++)
                    AI[L1, L2] = 0;
                AI[L2, L2] = 1;
            }

            for (LC = 0; LC < LL; LC++)
            {
                if (Math.Abs(Mat1[LC, LC]) < 0.0000000001)
                {
                    for (LCA = LC + 1; LCA < LL; LCA++)
                    {
                        if (LCA == LC) continue;
                        if (Math.Abs(Mat1[LC, LCA]) > 0.0000000001)
                        {
                            for (LCB = 0; LCB <= LL; LCB++)
                            {
                                Mat1[LCB, LC] = Mat1[LCB, LC] + Mat1[LCB, LCA];
                                AI[LCB, LC] = AI[LCB, LC] + AI[LCB, LCA];
                            }
                            break;
                        }
                    }
                }
                AIN = 1 / Mat1[LC, LC];
                for (LCA = 0; LCA < LL; LCA++)
                {
                    Mat1[LCA, LC] = AIN * Mat1[LCA, LC];
                    AI[LCA, LC] = AIN * AI[LCA, LC];
                }

                for (LCA = 0; LCA < LL; LCA++)
                {
                    if (LCA == LC) continue;
                    AF = Mat1[LC, LCA];
                    for (LCB = 0; LCB < LL; LCB++)
                    {
                        Mat1[LCB, LCA] = Mat1[LCB, LCA] - AF * Mat1[LCB, LC];
                        AI[LCB, LCA] = AI[LCB, LCA] - AF * AI[LCB, LC];
                    }
                }
            }
            return AI;
        }

        public static double[,] toMatrix(this double[][] a)
        {
            int rows = a.Length;
            int cols = a[0].Length;
            double[,] matrix = new double[rows, cols];

            for (int i = 0; i < rows; i++)
                for (int j = 0; j < cols; j++)
                    matrix[i, j] = a[i][j];
            return matrix;
        }

        public static double[][] to2D(this double[,] a)
        {
            int rows = a.GetLength(0);
            int cols = a.GetLength(1);
            double[][] matrix = new double[rows][];

            for (int i = 0; i < rows; i++)
            {
                matrix[i] = new double[cols];
                for (int j = 0; j < cols; j++)
                {
                    double num = a[i, j];
                    matrix[i][j] = num;
                }
            }
            return matrix;
        }


        #endregion

    }
}
